#include "ohos_init.h"          // For SYS_RUN
#include "cmsis_os2.h"          // For task management (osThreadNew, osDelay)
#include "hal_bsp_sht20.h"      // Include SHT20 driver header
#include "hal_bsp_ssd1306.h"    // Include SSD1306 driver header
#include <stdio.h>              // For snprintf, printf
#include <string.h>             // For memset
#include <unistd.h>             // For usleep (can be replaced by osDelay)

#define SENSOR_TASK_STACK_SIZE (1024 * 4) // 4KiB stack for the task
#define SENSOR_TASK_PRIO osPriorityNormal  // Task priority
#define UPDATE_INTERVAL_MS 2000           // Update display every 2000ms (2 seconds)
#define DISPLAY_BUFFER_SIZE 32            // Buffer size for formatting strings

static void SensorDisplayTask(void *arg)
{
    (void)arg; // Unused parameter

    float temp = 0.0f;
    float humi = 0.0f;
    uint32_t ret;
    char display_buf_temp[DISPLAY_BUFFER_SIZE];
    char display_buf_humi[DISPLAY_BUFFER_SIZE];

    // Initial display message
    SSD1306_CLS();
    SSD1306_ShowStr(0, 0, (uint8_t *)"SHT20 Reading...", 8); // Use small font for status

    osDelay(1000); // Wait a bit before starting loop

    while (1)
    {
        // Read data from SHT20 sensor
        ret = SHT20_ReadData(&temp, &humi);

        SSD1306_CLS(); // Clear display before writing new data

        if (ret == 0)
        {
            // Format the readings into strings
            snprintf(display_buf_temp, DISPLAY_BUFFER_SIZE, "Temp: %.1f C", temp);
            snprintf(display_buf_humi, DISPLAY_BUFFER_SIZE, "Humi: %.1f %%", humi); // Use %% for literal %

            // Display using 8x16 font (TextSize = 16)
            // Note: TextSize=16 uses two pages (y and y+1)
            SSD1306_ShowStr(0, 0, (uint8_t *)display_buf_temp, 16); // Display Temp on rows 0 & 1
            SSD1306_ShowStr(0, 2, (uint8_t *)display_buf_humi, 16); // Display Humi on rows 2 & 3

            printf("SHT20 Read: Temp=%.1fC, Humi=%.1f%%\r\n", temp, humi);
        }
        else
        {
            // Display error message
            snprintf(display_buf_temp, DISPLAY_BUFFER_SIZE, "SHT20 Error");
            snprintf(display_buf_humi, DISPLAY_BUFFER_SIZE, "Code: 0x%x", ret);
            SSD1306_ShowStr(0, 0, (uint8_t *)display_buf_temp, 16);
            SSD1306_ShowStr(0, 2, (uint8_t *)display_buf_humi, 16);
            printf("ERROR: Failed to read SHT20 sensor! Code: 0x%x\r\n", ret);
        }

        // Wait before next reading
        osDelay(UPDATE_INTERVAL_MS);
    }
}

static void SensorDisplayDemo(void)
{
    uint32_t ret;
    osThreadId_t taskId;

    printf("Starting Sensor Display Demo...\r\n");

    // Initialize SSD1306 (this also initializes I2C0 bus)
    ret = SSD1306_Init();
    if (ret != 0)
    {
        printf("FATAL: SSD1306 Init Failed! Error code: %u\r\n", ret);
        return; // Cannot proceed
    }
    printf("SSD1306 Initialized.\r\n");

    // Initialize SHT20 (device-specific part)
    ret = SHT20_Init();
    if (ret != 0)
    {
        // Display error on OLED if possible
        SSD1306_ShowStr(0, 5, (uint8_t *)"SHT20 Init ERR", 8);
        printf("ERROR: SHT20 Init Failed! Error code: %u\r\n", ret);
        // Continue anyway, task will show error
    }
    else
    {
        printf("SHT20 Initialized.\r\n");
    }


    osThreadAttr_t attr = {
        .name = "SensorTask",
        .attr_bits = 0U,
        .cb_mem = NULL,
        .cb_size = 0U,
        .stack_mem = NULL,
        .stack_size = SENSOR_TASK_STACK_SIZE,
        .priority = SENSOR_TASK_PRIO
    };

    taskId = osThreadNew(SensorDisplayTask, NULL, &attr);
    if (taskId == NULL)
    {
        printf("FATAL: Failed to create SensorDisplayTask!\r\n");
         // debug
        SSD1306_CLS();
        SSD1306_ShowStr(0, 0, (uint8_t *)"Task Create", 16);
        SSD1306_ShowStr(0, 2, (uint8_t *)"Failed!", 16);
    } else {
        printf("SensorDisplayTask created successfully.\r\n");
    }
}

SYS_RUN(SensorDisplayDemo);
